import lejos.nxt.*;

public class Balance {

	private static LightSensor ls = new LightSensor(SensorPort.S3);
	private static MotorPort leftMotor = MotorPort.A;
	private static MotorPort rightMotor = MotorPort.B;
	private static int value, error, power;
	private static double gain = 0.5;
	private static int minPower = 75;
	private static int desiredValue = 50;
	
	private final static int 	forward  = 1,
    							backward = 2,
								stop     = 3;
	
	public static void main(String [] args) throws Exception {
		while(!Button.ESCAPE.isPressed()) {
			value = ls.readValue();
			
			error = (value - desiredValue);		
			power = (int)(gain * (error));
			LCD.drawInt(power, 1, 2);
			LCD.drawInt(error, 4, 6);
			LCD.drawInt(value, 1, 3);
			Thread.sleep(25);
		
			
			if(Math.abs(error) < 5) {
				leftMotor.controlMotor(Math.max(power, minPower), stop);
				rightMotor.controlMotor(Math.max(power, minPower), stop);
				LCD.drawString("Stop", 1, 1);
				continue;
			} 
			
			if(error < 0) {
				
				leftMotor.controlMotor(Math.max(power, minPower), forward);
				rightMotor.controlMotor(Math.max(power, minPower), forward);
				LCD.drawString("In error", 1, 1);
				
			} else {
				
				leftMotor.controlMotor(65, backward);
				rightMotor.controlMotor(65, backward);
				LCD.drawString("No error", 1, 1);
				
			}
			Thread.sleep(25);
		}
		
	}
	
}
